#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include <cmath>
using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNavigationTopose = rclcpp_action::ClientGoalHandle<NavigateToPose>;
class  onegpoal : public  rclcpp::Node
{
private:
    rclcpp_action::Client<NavigateToPose>::SharedPtr nav_action_client_;
    geometry_msgs::msg::PoseStamped goal_pose_;
    GoalHandleNavigationTopose::SharedPtr goal_handle_;
    void send_goal()
    {
        auto goal_msgs=NavigateToPose::Goal();
        goal_msgs.pose=goal_pose_;
        auto send_goal_options= rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
        send_goal_options.goal_response_callback=std::bind(&onegpoal::goal_response_callback,this,std::placeholders::_1);
        send_goal_options.feedback_callback=std::bind(&onegpoal::feedback_callback,this,std::placeholders::_1,std::placeholders::_2);
        send_goal_options.result_callback=std::bind(&onegpoal::result_callback,this,std::placeholders::_1);
        nav_action_client_->async_send_goal(goal_msgs,send_goal_options);

    }
    void goal_response_callback(GoalHandleNavigationTopose::SharedPtr goal_handle)//目标响应
    {
        goal_handle_=goal_handle;//保存句柄
        if(!goal_handle)
        {
            RCLCPP_ERROR(this->get_logger(),"目标服务器拒绝");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(),"目标已经被接受");
        }

    }
    void feedback_callback(GoalHandleNavigationTopose::SharedPtr,const std::shared_ptr<const NavigateToPose::Feedback> feedback)//反馈回调
    {
        RCLCPP_INFO(this->get_logger(),"剩余距离：%.2f米",feedback->distance_remaining);
    }
    void result_callback(const GoalHandleNavigationTopose::WrappedResult & result)
    {
        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:RCLCPP_INFO(this->get_logger(),"成功到达目标点");
                rclcpp::shutdown();
            break;
        case rclcpp_action::ResultCode::ABORTED:RCLCPP_ERROR(this->get_logger(),"导航失败");
                rclcpp::shutdown();
            break;
        case rclcpp_action::ResultCode::CANCELED:RCLCPP_WARN(this->get_logger(),"成功到达目标点");
                rclcpp::shutdown();
            break;
        default:
             RCLCPP_ERROR(this->get_logger(),"未知结果");
            break;
        }
    }
    

public:
    explicit onegpoal():Node("onegoal")
    {
        //位置参数有待修改
        goal_pose_.header.frame_id="map";
        goal_pose_.pose.position.x=2.5;
        goal_pose_.pose.position.y=1.8;
        goal_pose_.pose.position.z=0.0;
        tf2::Quaternion q;
        q.setRPY(0,0,0);//设置三个旋转角都为0
        goal_pose_.pose.orientation=tf2::toMsg(q);
        //创建客户端
        nav_action_client_=rclcpp_action::create_client<NavigateToPose>(this,"navigate_to_pose");
        if(!nav_action_client_->wait_for_action_server(std::chrono::seconds(10)))
        {
            RCLCPP_ERROR(this->get_logger(),"导航服务器超时未响应");
            rclcpp::shutdown();
            return ;
        }

    }

};
int main(int argc,char **argv)
{
    rclcpp::init(argc,argv);
    auto node = std::make_shared<onegpoal>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;

}